#include "RayDirectionPicking.h"
#include "SharedConstants.h"
#include "HeightMapCoordinates.h"
#include "TransformCoords.h"

std::pair<TTriangle3,TTriangle3> GetTrianglesByCell (const Cell& i_cell, const Heights& i_heights)
  {
  auto coords = HeightMapUtils::ConvertHMapPointToTVector(i_cell.first, i_cell.second, std::get<0> (i_heights));
  double x = coords[0];
  double y = coords[1];
  double z_ul = std::get<0> (i_heights) * GroundConstants::CELL_SIZE;
  double z_ur = std::get<1> (i_heights) * GroundConstants::CELL_SIZE;
  double z_bl= std::get<2> (i_heights) * GroundConstants::CELL_SIZE;
  double z_br = std::get<3> (i_heights) * GroundConstants::CELL_SIZE;

  TVector3 up_left (x, y, z_ul);
  TVector3 up_right (x + GroundConstants::CELL_SIZE, y, z_ur);
  TVector3 bottom_left (x, y + GroundConstants::CELL_SIZE, z_bl);
  TVector3 bottom_right (x + GroundConstants::CELL_SIZE, y + GroundConstants::CELL_SIZE, z_br);
  TTriangle3 up (up_right, up_left, bottom_left);
  TTriangle3 down (bottom_right, up_right, bottom_left);

  return std::make_pair(up, down);
  };

std::vector<TTriangle3> PickTrianglesWithCellsAndLine (const HeightMap & i_map, const TLine3& i_line)
  {
  TRay2 projection = GetProjectionOnXY(i_line);
  auto cells = PickNeededCells(i_map, projection);
  std::vector<TTriangle3> result;
  for( size_t i = 0; i < cells.size(); i ++)
    {
    Heights heights (i_map.Get(cells[i].first, cells[i].second), i_map.Get(cells[i].first + 1, cells[i].second),
                     i_map.Get(cells[i].first, cells[i].second + 1), i_map.Get(cells[i].first + 1, cells[i].second + 1));
    auto pair_triangles = GetTrianglesByCell(cells[i], heights);
    TRay3 ray3 (i_line.Origin, i_line.Direction);
    if (IntersectionTest(pair_triangles.first, ray3))
      result.push_back(pair_triangles.first);
    if (IntersectionTest(pair_triangles.second, ray3))
      result.push_back(pair_triangles.second);
    }
  return result;
  };


TBox2 GetBoxForBorder(const HeightMap& i_map)
  {
  double ext_x= ((i_map.GetSizeX() - 1) * GroundConstants::CELL_SIZE) / 2.0;
  double ext_y= ((i_map.GetSizeY() - 1) * GroundConstants::CELL_SIZE) / 2.0;
  TVector2 box_origin (GroundConstants::ORIGIN_X + ext_x, GroundConstants::ORIGIN_Z + ext_y);
  TVector2 u0 (1.0, 0.0);
  TVector2 u1 (0.0, 1.0);
  return TBox2 (box_origin, u0, u1, ext_x, ext_y);
  };

bool DirectionXNullCase (const HeightMap& i_map, const TRay2& i_ray, Cell& i_indexes, std::vector<Cell>& o_result)
  {
  if (i_ray.Direction.X() == 0)
    {
    if (i_indexes.first >= i_map.GetSizeX() - 1)
        i_indexes.first -= 1;
    if (i_ray.Direction.Y() == 0)
      {
      if (i_indexes.second >= i_map.GetSizeY() - 1)
        i_indexes.second -= 1;
      o_result.push_back(std::make_pair(i_indexes.first, i_indexes.second));
      return true;
      }
    else
      {
      int y_rise;
      if (i_ray.Direction.Y() < 0)
        y_rise = -1;
      else
        y_rise = 1;
      if (i_indexes.second == i_map.GetSizeY() - 1 && y_rise < 0)
        i_indexes.second -= 1;
      auto j = i_indexes.second; 
      while (j >= 0 && j <i_map.GetSizeY() - 1)
        {
        o_result.push_back(std::make_pair(i_indexes.first, j));
        j += y_rise;
        }
      return true;
      }
    }
  return false;
  };
// some wrapper on Brizingham algorithm
std::vector<Cell> PickNeededCells (const HeightMap& i_map, const TRay2& i_ray)
  {
  std::vector<Cell> result;
  TBox2 ground_border = GetBoxForBorder(i_map);
  TVector2 intersection_point;
  if (!GetIntersectionPoint (intersection_point, ground_border, i_ray))
    return result;
  auto indexes= HeightMapUtils::FindHeightMapIndexes(intersection_point, TVector2 (GroundConstants::ORIGIN_X, GroundConstants::ORIGIN_Z), 
                                                     GroundConstants::CELL_SIZE);
  if (DirectionXNullCase(i_map, i_ray, indexes, result))
    return result;
  
  // conducting an increment of x and y iterators of the cells, concerning the direction of the ray
  int x_rise, y_rise;
  if (i_ray.Direction.X() < 0)
    x_rise = -1;
  else
    x_rise = 1;
  
  if (i_ray.Direction.Y() < 0)
    y_rise = -1;
  else
    y_rise = 1;
  
  //calculating angle and some other values
  double x0 = intersection_point.X();
  double y0 = intersection_point.Y();
  size_t x_prev = indexes.first;
  if (i_ray.Direction.X() < 0 && x_prev < i_map.GetSizeX() - 1 )
    x_prev += 1;
  size_t y_prev = indexes.second;
  double ctg_alpha = std::fabs(i_ray.Direction.X()) / i_ray.Direction.Y();
  size_t x_next, y_next;
  
  //calculating result using the wrapper to Brizingham algo
  while (y_prev <= i_map.GetSizeY() - 1  && y_prev >= 0)
    {
    x_next = x_prev + x_rise;
    if (x_next > i_map.GetSizeX() - 1 || x_next < 0)
      break;
    TVector2 next_intersection (x_next * GroundConstants::CELL_SIZE, y0 + std::fabs(x_next * GroundConstants::CELL_SIZE- x0) / ctg_alpha);
    y_next = HeightMapUtils::FindHeightMapIndexes(next_intersection, TVector2 (GroundConstants::ORIGIN_X, GroundConstants::ORIGIN_Z),
                                                  GroundConstants::CELL_SIZE).second;
    for (auto yy = y_prev; yy != y_next + y_rise; yy += y_rise)
      {
      if (yy < i_map.GetSizeY() - 1 && yy >= 0)
        if (i_ray.Direction.X() > 0)
          result.push_back(std::make_pair(x_prev, yy));
        else
          result.push_back(std::make_pair(x_next, yy));
      }
    x_prev = x_next;
    y_prev = y_next;
    }
  
  return result;
  };
